import sys
print("当前 sys.path 前几项：")
for p in sys.path[:6]:
    print(p)

import numpy as np
print("NumPy 实际路径：", np.__file__)
print("NumPy 版本：", np.__version__)

import threading
print("threading 文件路径:", threading.__file__)
print("是否有 _set_sentinel:", hasattr(threading, '_set_sentinel'))

import time
# import threading
import serial
# import sys
import os

# ==================== 【关键修复】把路径和 import 提到最前面 ====================
# 立即添加当前目录到 Python 模块搜索路径
current_dir = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, current_dir)

# 现在再导入 BrainLinkParser（必须在 sys.path 修改之后）
try:
    from BrainLinkParser import BrainLinkParser
    print("✅ BrainLinkParser 模块加载成功！")
except ImportError as e:
    print("❌ 无法导入 BrainLinkParser！")
    print("   可能原因：")
    print("   1. Python 版本不是 3.11（官方要求必须 3.11）")
    print("   2. BrainLinkParser.pyd 文件不在当前文件夹")
    print("   3. 文件名不是 BrainLinkParser.pyd（注意大小写）")
    print(f"   当前 Python 版本: {sys.version}")
    print(f"   当前搜索路径: {sys.path[0]}")
    sys.exit(1)
# ===========================================================================

from djitellopy import Tello

# ==================== 配置区 ====================
BRAIN_PORT = "COM6"          # ←←←←← 这里改成你 Windows 的实际输出端口（很重要！）
                             # 通常是 COM3、COM5、COM7 等，建议在设备管理器里确认是“输出”端口

ATTENTION_TAKEOFF = 85
MEDITATION_LAND = 85
FORWARD_THRESHOLD = 75
BACKWARD_THRESHOLD = 75
FLIP_LOW_BETA = 78
FLIP_HIGH_BETA = 78
CIRCLE_HIGH_GAMMA = 82

DEBOUNCE_SEC = 3.0
RC_INTERVAL = 0.08
# ===============================================

# 全局变量...
latest_data = None
data_lock = threading.Lock()
is_flying = False
circling = False
last_action_time = 0
last_flip_time = 0
last_rc_time = 0

def on_eeg(data):
    global latest_data
    with data_lock:
        latest_data = data
    print(f"🧠 脑波 | 注意力:{data.attention:3d} 冥想:{data.meditation:3d} | "
          f"lowβ:{data.lowBeta:3d} highβ:{data.highBeta:3d} highγ:{data.highGamma:3d}")

def on_extend_eeg(data):
    print(f"📡 扩展 | 电量:{data.battery}%")

def on_gyro(x, y, z): pass
def on_rr(rr1, rr2, rr3): pass
def on_raw(raw): pass

# BrainLink 串口读取线程
def serial_reading_thread(port):
    global parser
    try:
        ser = serial.Serial(port, 115200, timeout=1)
        print(f"✅ BrainLink 串口已打开: {port}")
        
        parser = BrainLinkParser(on_eeg, on_extend_eeg, on_gyro, on_rr, on_raw)
        
        while True:
            if ser.in_waiting > 0:
                byte_data = ser.read(ser.in_waiting)
                if byte_data:
                    parser.parse(byte_data)
            time.sleep(0.01)
    except Exception as e:
        print(f"❌ 串口错误: {e}")
        sys.exit(1)

if __name__ == "__main__":
    # 启动 BrainLink 读取线程
    threading.Thread(target=serial_reading_thread, args=(BRAIN_PORT,), daemon=True).start()
    
    # 初始化 Tello
    tello = Tello()
    tello.connect()
    print(f"✅ Tello 已连接，当前电量: {tello.get_battery()}%")
    
    print("\n🚀 增强版脑控无人机已就绪！")
    print("Ctrl+C 安全退出\n")
    
    try:
        while True:
            # ...（后面控制逻辑和之前完全一样，这里省略以节省篇幅）
            # 你可以直接把之前增强版代码的 while True 部分粘贴进来
            current_time = time.time()
            with data_lock:
                if latest_data is None:
                    time.sleep(0.1)
                    continue
                d = latest_data
                att = d.attention
                med = d.meditation
                low_beta = d.lowBeta
                high_beta = d.highBeta
                high_gamma = d.highGamma

            if current_time - last_action_time > DEBOUNCE_SEC:
                if att >= ATTENTION_TAKEOFF and not is_flying:
                    print("🛫 高注意力 → 执行【起飞】")
                    tello.takeoff()
                    is_flying = True
                    circling = False
                    last_action_time = current_time
                elif med >= MEDITATION_LAND and is_flying and not circling:
                    print("🛬 高冥想 → 执行【降落】")
                    tello.land()
                    is_flying = False
                    circling = False
                    last_action_time = current_time

            if is_flying and current_time - last_rc_time > RC_INTERVAL:
                fb = 0
                yaw = 0
                if att > FORWARD_THRESHOLD:
                    fb = min(int((att - FORWARD_THRESHOLD) * 1.8), 70)
                elif med > BACKWARD_THRESHOLD:
                    fb = max(-int((med - BACKWARD_THRESHOLD) * 1.6), -60)
                if circling:
                    yaw = 45
                    fb = max(fb, 25)
                tello.send_rc_control(0, fb, 0, yaw)
                last_rc_time = current_time

            if is_flying and current_time - last_flip_time > DEBOUNCE_SEC + 1:
                if low_beta >= FLIP_LOW_BETA:
                    print("🔄 lowBeta 高 → 执行【左翻滚】")
                    tello.flip_left()
                    last_flip_time = current_time
                elif high_beta >= FLIP_HIGH_BETA:
                    print("🔄 highBeta 高 → 执行【右翻滚】")
                    tello.flip_right()
                    last_flip_time = current_time

            if is_flying and high_gamma >= CIRCLE_HIGH_GAMMA and current_time - last_action_time > DEBOUNCE_SEC:
                circling = not circling
                print(f"{'🌀 启动【绕飞模式】' if circling else '⭕ 停止绕飞'}")
                last_action_time = current_time

            time.sleep(0.05)

    except KeyboardInterrupt:
        print("\n⛔ 用户中断 → 安全停止...")
        if is_flying:
            tello.send_rc_control(0, 0, 0, 0)
            try:
                tello.land()
            except:
                pass
        print("程序已安全退出！")